package pl.polsl.mmm.projectcomm.data;

import java.io.Serializable;

/**
 * Created by Maciek on 2014-11-02.
 */
public class vec4 implements Serializable {
    public float x,y,z,w;
    //statyczne zmienne pomocnicze
    private static vec4 temp = new vec4();
    private static float c;
    private static float k0,k1, s, o ,o1;
    /******************** KONSTRUKTORY *********************************/
    public vec4() {
        x = y = z = w = 0f;
    }
    public vec4 (vec3 v1) {
        this.x = v1.x;
        this.y = v1.y;
        this.z = v1.z;
    }
    public vec4 (vec4 v1) {
        this.x = v1.x;
        this.y = v1.y;
        this.z = v1.z;
        this.w = v1.w;
    }
    public vec4(float x, float y, float z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }
    public vec4(float x, float y, float z, float w) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.w = w;
    }

    /********************** METODY *************************************/
    /** Wyznaczenie pola w wektora na podstawie pozostałych pól*/
    public void buildW() {
        c = 1.0f - ( x * x ) -
                ( y * y ) -
                ( z * z );

        w = ( c < 0.0f ) ? 0.0f : -(float)Math.sqrt(c);
    }
    /** Kopiowanie wektora bez konieczności tworzenia nowego obiektu */
    public void copy(vec4 v1) {
        this.x = v1.x;
        this.y = v1.y;
        this.z = v1.z;
        this.w = v1.w;
    }
    /** Normalizacja wektora 4-elem*/
    public void normalize()
    {
        c = length();
        if(c != 0f) {
            o = 1.0f / c;
            x *= o;
            y *= o;
            z *= o;
            w *= o;
        }
    }
    /** Iloczyn skalarny wektorów 4-elem. */
    public float dotProduct(vec4 other)
    {
        return ( x * other.x ) +
                ( y * other.y ) +
                ( z * other.z ) +
                ( w * other.w );
    }
    /** Długość wektora 4-elem. */
    float length()
    {
        return (float)Math.sqrt( dotProduct(this) );
    }

    /** Iloczyn wektorowy wektorów: 4- i 3-elem. */
    public void multiplyVec3( vec4 v0, vec3 v1)
    {
        x =  ( v0.w * v1.x ) + ( v0.y * v1.z ) - ( v0.z * v1.y );
        y =  ( v0.w * v1.y ) + ( v0.z * v1.x ) - ( v0.x * v1.z );
        z =  ( v0.w * v1.z ) + ( v0.x * v1.y ) - ( v0.y * v1.x );
        w = -( v0.x * v1.x ) - ( v0.y * v1.y ) - ( v0.z * v1.z );
    }
    /** Iloczyn wektorowy wektorów 4-elem. */
    public void multiplyVec4( vec4 v0, vec4 v1 )
    {

        x = ( v0.x * v1.w ) + ( v0.w * v1.x ) + ( v0.y * v1.z ) - ( v0.z * v1.y );
        y = ( v0.y * v1.w ) + ( v0.w * v1.y ) + ( v0.z * v1.x ) - ( v0.x * v1.z );
        z = ( v0.z * v1.w ) + ( v0.w * v1.z ) + ( v0.x * v1.y ) - ( v0.y * v1.x );
        w = ( v0.w * v1.w ) - ( v0.x * v1.x ) - ( v0.y * v1.y ) - ( v0.z * v1.z );
    }
    /** Sprzężenie wektora 4-elem. ( inwersja bez elementu w ) */
    public void conjugate( vec4 v )
    {
        x = -v.x;
        y = -v.y;
        z = -v.z;
        w = v.w;
    }
    /** Inwersja wektora 4-elem. */
    public void invert(vec4 v) {
        x = -v.x;
        y = -v.y;
        z = -v.z;
        w = -v.w;
    }
    /** Interpolacja liniowa pomiędzy dwoma wektorami 4-elem.
     * t == 1f oznacza 100% udział wektora v1 i 0% wektora v0
     * t == 0f oznacza 100% udział wektora v0 i 0% wektora v1
     * */
    public void lerp(vec4 v0, vec4 v1, float t) {
        if( t==1.0f ) {
            this.copy(v1);
            return;
        } else if (t==0.0f) {
            this.copy(v0);
            return;
        }
        x = v0.x + t * (v1.x - v0.x);
        y = v0.y + t * (v1.y - v0.y);
        z = v0.z + t * (v1.z - v0.z);
        w = v0.w + t * (v1.w - v0.w);
    }
    /** Sferyczna interpolacja liniowa pomiędzy dwoma wektorami 4-elem.
     * t == 1f oznacza 100% udział wektora v1 i 0% wektora v1
     * t == 0f oznacza 100% udział wektora v0 i 0% wektora v0
     * */
    public void slerp(vec4 v0, vec4 v1, float t) {
        c = v0.dotProduct(v1);
        if(t == 1f) {
            this.copy(v1);
            return;
        } else if( t==0f) {
            this.copy(v0);
            return;
        }
        temp.copy(v1);

        if ( c < 0f ){
            temp.invert(v1);
            c = -c;
        }
        if ( c > 0.999999f ) {
            k0 = 1f - t;
            k1 = t;
        } else {
            s = (float)Math.sqrt(1f - (c*c));
            o = (float)Math.atan2(s,c);
            o1 = 1f / s;

            k0 = (float)Math.sin((1f - t)*o)*o1;
            k1 = (float)Math.sin(t*o) * o1;
        }
        //interpolacja
        x = (k0 * v0.x) + (k1 * temp.x);
        y = (k0 * v0.y) + (k1 * temp.y);
        z = (k0 * v0.z) + (k1 * temp.z);
        w = (k0 * v0.w) + (k1 * temp.w);
    }
    /** Porównanie wektora z innym. Zwraca true jeśli są identyczne*/
    public static boolean vec4Compare(vec4 v0, vec4 v1) {
        if(v0.x==v1.x && v0.y==v1.y && v0.z==v1.z && v0.w==v1.w) {
            return true;
        }
        else
            return false;
    }

    public static vec4 quaternionRotationX(float angle)
    {
        vec4 newQuat = new vec4();
        newQuat.x = (float)Math.sin(angle/2f);
        newQuat.y = 0.0f;
        newQuat.z = 0.0f;
        newQuat.w = (float)Math.cos(angle/2f);

        return newQuat;
    }

    public static vec4 quaternionRotationY(float angle)
    {
        vec4 newQuat = new vec4();
        newQuat.x = 0.0f;
        newQuat.y = (float)Math.sin(angle/2f);
        newQuat.z = 0.0f;
        newQuat.w = (float)Math.cos(angle/2f);

        return newQuat;
    }

    public static vec4 quaternionRotationZ(float angle)
    {
        vec4 newQuat = new vec4();
        newQuat.x = 0.0f;
        newQuat.y = 0.0f;
        newQuat.z = (float)Math.sin(angle/2f);
        newQuat.w = (float)Math.cos(angle/2f);

        return newQuat;
    }

    public static vec4 calculateAngle(final vec3 center ,final vec3 newPos,final vec3 previousPos) {
        vec3 prevPosTranslated = new vec3();
        vec3 newPosTranslated = new vec3();

        prevPosTranslated.diff(previousPos,center);
        newPosTranslated.diff(newPos,center);
        prevPosTranslated.normalize();
        newPosTranslated.normalize();

        float angle1 = (float)Math.atan2(newPosTranslated.z,newPosTranslated.x);
        float angle2 = (float)Math.atan2(prevPosTranslated.z,prevPosTranslated.x);

        float finalAngle = Math.abs(angle1) - Math.abs(angle2);
        return vec4.quaternionRotationY(finalAngle);
    }

    /** Konwersja kwaternionu na macierz obrotu */
    public float[] quaternionToRotationMatrix() {
        normalize();
        float matrix[] = {
                1.0f - 2.0f*y*y - 2.0f*z*z,
                2.0f*x*y - 2.0f*z*w,
                2.0f*x*z + 2.0f*y*w,
                0.0f,
                2.0f*x*y + 2.0f*z*w,
                1.0f - 2.0f*x*x - 2.0f*z*z,
                2.0f*y*z - 2.0f*x*w,
                0.0f,
                2.0f*x*z - 2.0f*y*w,
                2.0f*y*z + 2.0f*x*w,
                1.0f - 2.0f*x*x - 2.0f*y*y,
                0.0f,
                0.0f,
                0.0f,
                0.0f,
                1.0f };
        return matrix;
    }

    public static float[][] getMatrixFromRotationAroundPoint(vec4 q,vec3 centre) {
        float[][] rotationMatrix = new float[4][4];
        float sqw = q.w*q.w;
        float sqx = q.x*q.x;
        float sqy = q.y*q.y;
        float sqz = q.z*q.z;
        rotationMatrix[0][0] = sqx - sqy - sqz + sqw; // since sqw + sqx + sqy + sqz =1
        rotationMatrix[1][1] = -sqx + sqy - sqz + sqw;
        rotationMatrix[2][2] = -sqx - sqy + sqz + sqw;

        float tmp1 = q.x*q.y;
        float tmp2 = q.z*q.w;
        rotationMatrix[0][1] = 2f * (tmp1 + tmp2);
        rotationMatrix[1][0] = 2f * (tmp1 - tmp2);

        tmp1 = q.x*q.z;
        tmp2 = q.y*q.w;
        rotationMatrix[0][2] = 2f * (tmp1 - tmp2);
        rotationMatrix[2][0] = 2f * (tmp1 + tmp2);

        tmp1 = q.y*q.z;
        tmp2 = q.x*q.w;
        rotationMatrix[1][2] = 2f * (tmp1 + tmp2);
        rotationMatrix[2][1] = 2f * (tmp1 - tmp2);

        float a1,a2,a3;
        if (centre == null) {
            a1=a2=a3=0;
        } else {
            a1 = centre.x;
            a2 = centre.y;
            a3 = centre.z;
        }
        rotationMatrix[0][3] = a1 - a1 * rotationMatrix[0][0] - a2 * rotationMatrix[0][1] - a3 * rotationMatrix[0][2];
        rotationMatrix[1][3] = a2 - a1 * rotationMatrix[1][0] - a2 * rotationMatrix[1][1] - a3 * rotationMatrix[1][2];
        rotationMatrix[2][3] = a3 - a1 * rotationMatrix[2][0] - a2 * rotationMatrix[2][1] - a3 * rotationMatrix[2][2];
        rotationMatrix[3][0] = rotationMatrix[3][1] = rotationMatrix[3][2] = 0f;
        rotationMatrix[3][3] = 1f;

        return rotationMatrix;
    }
    public static vec4 get_rotation_between(final vec3 from, final vec3 to)
    {
        float k_cos_theta = from.dotProduct(to);
        float wq = (float)Math.sqrt(from.length_2() * to.length_2()) + k_cos_theta;
        if (wq < 0.0001)
        {
            return new vec4(-from.z,from.y, from.x,0);
        }
        vec3 cross = new vec3();
        cross.crossProduct(from,to);
        vec4 newRotation = new vec4(cross);
        newRotation.w = wq;
        newRotation.normalize();
        return newRotation;
    }
}
